package org.maxgroundstation.groundstation.comm;

import java.io.IOException;
import java.net.DatagramPacket;
import java.net.DatagramSocket;
import java.net.InetAddress;

import org.maxgroundstation.groundstation.GroundStation;

/**
 * Class for sending commands to the MAX robot.
 * @author Scott Davis
 * 
 * Copyright (C) 2010  Scott Davis
 * 
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
public class CommandSender implements Runnable {
    
    // Default port
    public static final int DEFAULT_PORT = 10301;
	
    // Length of alive thread sleep time (milliseconds).
    private static final long SLEEP_TIME = 100L;
    
    // MAX robot UDP command types
    public static final int MOTOR_COMMAND = 2;
//    public static final int TURN_COMMAND = 3;
    public static final int TURN_FRONT_COMMAND = 4;
    public static final int TURN_REAR_COMMAND = 5;
    public static final int PAN_COMMAND = 6;
    public static final int TILT_COMMAND = 7;
    public static final int IM_ALIVE_COMMAND = 0;
    public static final int TEXT_COMMAND = 1;
    
    // Data members
    private DatagramSocket socket;
    private String ipAddress;
    private int port;
    private Thread aliveThread;
    private boolean run;
    private GroundStation station;
    private int commandIterator;
    private boolean sendInitialVideoParams;
    private int videoWidthCache;
    private int videoHeightCache;
    private int videoFpsCache;
    
    /**
     * Constructor
     * @param station the ground station.
     */
    public CommandSender(GroundStation station) {
        ipAddress = "";
        port = 0;
        this.station = station;
        commandIterator = 0;
        sendInitialVideoParams = false;
        videoWidthCache = 0;
        videoHeightCache = 0;
        videoFpsCache = 0;
        
        try {
            socket = new DatagramSocket();
        }
        catch (IOException e) {
            System.err.println("Error opening datagram socket: " + e.getMessage());
        }
        
        run = true;
        
        start();
    }
    
    /**
     * Sets the refresh thread to run or not.
     * @param run true if panel is to be refreshed.
     */
    public void setRun(boolean run) {
        this.run = run;
    }
    
    /**
     * Starts the update thread.
     */
    public void start() {
        if ((aliveThread == null) || !run) { 
            aliveThread = new Thread(this, "I'm alive thread");
            aliveThread.start();
        }
    }
    
    /**
     * Run the update thread.
     */
    public void run() {
    	run = true;
        while (run) {
            try {
                Thread.sleep(SLEEP_TIME);
            } 
            catch (InterruptedException e) {}
            
            if (isConnected()) {
                commandIterator++;
                if (commandIterator >= 1000) commandIterator = 0;
                sendUDPCommand(IM_ALIVE_COMMAND, 0F);
                
                if (sendInitialVideoParams) {
                    sendInitialVideoParamsCommand();
                    pan(0F);
                    tilt(0F);
                    sendInitialVideoParams = false;
                }
            }
        }
    }
    
    /**
     * Connect to the robot via UDP.
     * @throws IOException if error connecting to robot.
     */
    public void connectUDP() throws IOException {
        if (!socket.isConnected() || socket.isClosed()) {
            System.out.println("Connecting command UDP");
            InetAddress address = InetAddress.getByName(ipAddress);
            if (socket.isClosed()) socket = new DatagramSocket();
            socket.connect(address, port);
            sendInitialVideoParams = true;
        }
    }
    
    /**
     * Disconnect UDP connection from the robot.
     * @throws IOException if error disconnecting from robot.
     */
    public void disconnectUDP() throws IOException {
        if (socket.isConnected() && !socket.isClosed()) {
            System.out.println("Disconnecting UDP command socket");
            socket.close();
        }
    }
    
    /**
     * Checks if connected to robot.
     * @return true if connected
     */
    public boolean isConnected() {
        return socket.isConnected() && !socket.isClosed();
    }
    
    /**
     * Gets the current IP address.
     * @return IP address as string.
     */
    public String getIPAddress() {
        return ipAddress;
    }
    
    /**
     * Sets the IP address.
     * @param ipAddress the new IP address.
     */
    public void setIPAddress(String ipAddress) {
        this.ipAddress = ipAddress;
    }
    
    /**
     * Gets the current port number.
     * @return port number.
     */
    public int getPort() {
        return port;
    }
    
    /**
     * Sets the port number.
     * @param port the new port number.
     */
    public void setPort(int port) {
        this.port = port;
    }
    
    /**
     * Send command for turning the front wheels.
     * @param turnAngle the turn angle (radians).
     */
    public void turnFront(float turnAngle) {
        sendUDPCommand(TURN_FRONT_COMMAND, turnAngle);
    }
    
    /**
     * Send command for turning the rear wheels.
     * @param turnAngle the turn angle (radians).
     */
    public void turnRear(float turnAngle) {
        sendUDPCommand(TURN_REAR_COMMAND, 0F - turnAngle);
    }
    
    /**
     * Send command for turning the wheels with anti-roll feature.
     * @param turnAngle the turn angle (radians).
     */
    public void turnAntiRoll(float turnAngle) {
//        sendUDPCommand(TURN_COMMAND, turnAngle);
        sendUDPCommand(TURN_FRONT_COMMAND, turnAngle);
    	sendUDPCommand(TURN_REAR_COMMAND, turnAngle);
    }
    
    /**
     * Send command for driving the axle motors.
     * @param power drive power (1.0 to - 1.0).
     */
    public void driveMotor(float power) {
        sendUDPCommand(MOTOR_COMMAND, power);
    }
    
    /**
     * Send command for panning the camera turret.
     * @param panAngle the pan angle (radians).
     */
    public void pan(float panAngle) {
        sendUDPCommand(PAN_COMMAND, panAngle);
    }
    
    /**
     * Send command for tilting the camera turret.
     * @param tiltAngle the tile angle (radians).
     */
    public void tilt(float tiltAngle) {
        sendUDPCommand(TILT_COMMAND, tiltAngle);
    }
    
    /**
     * Send the video params text command.
     * @param width the width of the requested video image.
     * @param height the height of the requested video image.
     * @param fps the request video frames per second.
     */
    public void videoParams(int width, int height, int fps) {
    	
    	videoWidthCache = width;
    	videoHeightCache = height;
    	videoFpsCache = fps;
    	
    	// Use lower bandwidth setting if set.
    	if (station.useLowBandwidthVideo()) fps = (int) Math.ceil((double) fps / 4D);
    	
        StringBuffer textBuff = new StringBuffer("videoparams:");
        textBuff.append(width + ",");
        textBuff.append(height + ",");
        textBuff.append(fps);
        sendUDPCommand(TEXT_COMMAND, textBuff.toString());
    }
    
    /**
     * Resends the video params text command with the current video settings.
     */
    public void updateVideoParams() {
    	videoParams(videoWidthCache, videoHeightCache, videoFpsCache);
    }
    
    /**
     * Gets the command iterator as a three-digit number string.
     * @return command iterator string.
     */
    private String getCommandIteratorString() {
        StringBuffer result = new StringBuffer("");
        if (commandIterator < 100) result.append("0");
        if (commandIterator < 10) result.append("0");
        result.append(commandIterator);
        return result.toString();
    }
    
    /**
     * Send a UDP command packet to the robot.
     * @param commandType the command type.
     * @param commandValue the command value.
     */
    public void sendUDPCommand(int commandType, float commandValue) {
        sendUDPCommand(commandType, commandValue, null); 
    }
    
    /**
     * Send a UDP command packet to the robot.
     * @param commandType the command type.
     * @param textValue the command text value.
     */
    public void sendUDPCommand(int commandType, String textValue) {
        sendUDPCommand(commandType, 0F, textValue);
    }
    
    /**
     * Send a UDP command packet to the robot.
     * @param commandType the command type.
     * @param commandValue the command value.
     * @param textValue the text value or null if none.
     */
    public void sendUDPCommand(int commandType, float commandValue, String textValue) {
        StringBuffer commandStringBuff = new StringBuffer();
        commandStringBuff.append(commandType);
        commandStringBuff.append(getCommandIteratorString());
        if (textValue != null) commandStringBuff.append(textValue);
        else commandStringBuff.append(commandValue);
        
        byte data[] = commandStringBuff.toString().getBytes();
        
        if (isConnected()) {
            try {
                DatagramPacket packet = new DatagramPacket(data, data.length);
                InetAddress address = InetAddress.getByName(ipAddress);
                packet.setAddress(address);
                packet.setPort(port);
                //System.out.println("Sending UDP command: " + commandStringBuff.toString());
                socket.send(packet);
            }
            catch (IOException e) {
                System.err.println("Error sending command UDP packet: " + e.getMessage());
            }
        }
    }
    
    /**
     * Sends the initial video parameters command.
     */
    private void sendInitialVideoParamsCommand() {
    	int width = 320;
    	int height = 240;
    	int fps = 30;
    	videoParams(width, height, fps);
    }
}